#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "std_srvs/srv/trigger.hpp"

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

class LifecycleServiceServer : public rclcpp_lifecycle::LifecycleNode
{
public:
    LifecycleServiceServer() : LifecycleNode("lifecycle_service_server") {}

    CallbackReturn on_configure(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_configure() called");
        srv_ = create_service<std_srvs::srv::Trigger>(
            "lifecycle_service", std::bind(&LifecycleServiceServer::handle_service, this, std::placeholders::_1, std::placeholders::_2));
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_activate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_activate() called");
        // 在激活时可以添加一些确认逻辑
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_deactivate() called");
        return CallbackReturn::SUCCESS;
    }

private:
    void handle_service(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
                        std::shared_ptr<std_srvs::srv::Trigger::Response> response)
    {
        RCLCPP_INFO(get_logger(), "Service called: responding with success");
        response->success = true;
        response->message = "Service response message";
    }


    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<LifecycleServiceServer>();

    // 使用生命周期管理
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node->get_node_base_interface());
    executor.spin();

    rclcpp::shutdown();
    return 0;
}
